#include "sbus.h"

uint8_t sbus_buffer[25] = {0};
uint8_t buffer_index = 0;

uint8_t sbus_parse_char(uint8_t rx, uint16_t* channels)
{
	if (buffer_index == 0 && rx != SBUS_STARTBYTE) {
		return SBUS_ERROR_FRAME;
	}

	sbus_buffer[buffer_index++] = rx;

	if (buffer_index == 25) {
		buffer_index = 0;
		if (sbus_buffer[24] != SBUS_ENDBYTE) {
			//incorrect end byte, out of sync
			return SBUS_ERROR_FRAME;
		}

		channels[0]  = ((sbus_buffer[1]    |sbus_buffer[2]<<8)                 & 0x07FF);
		channels[1]  = ((sbus_buffer[2]>>3 |sbus_buffer[3]<<5)                 & 0x07FF);
		channels[2]  = ((sbus_buffer[3]>>6 |sbus_buffer[4]<<2 |sbus_buffer[5]<<10)  & 0x07FF);
		channels[3]  = ((sbus_buffer[5]>>1 |sbus_buffer[6]<<7)                 & 0x07FF);
		channels[4]  = ((sbus_buffer[6]>>4 |sbus_buffer[7]<<4)                 & 0x07FF);
		channels[5]  = ((sbus_buffer[7]>>7 |sbus_buffer[8]<<1 |sbus_buffer[9]<<9)   & 0x07FF);
		channels[6]  = ((sbus_buffer[9]>>2 |sbus_buffer[10]<<6)                & 0x07FF);
		channels[7]  = ((sbus_buffer[10]>>5|sbus_buffer[11]<<3)                & 0x07FF);
		channels[8]  = ((sbus_buffer[12]   |sbus_buffer[13]<<8)                & 0x07FF);
		channels[9]  = ((sbus_buffer[13]>>3|sbus_buffer[14]<<5)                & 0x07FF);
		channels[10] = ((sbus_buffer[14]>>6|sbus_buffer[15]<<2|sbus_buffer[16]<<10) & 0x07FF);
		channels[11] = ((sbus_buffer[16]>>1|sbus_buffer[17]<<7)                & 0x07FF);
		channels[12] = ((sbus_buffer[17]>>4|sbus_buffer[18]<<4)                & 0x07FF);
		channels[13] = ((sbus_buffer[18]>>7|sbus_buffer[19]<<1|sbus_buffer[20]<<9)  & 0x07FF);
		channels[14] = ((sbus_buffer[20]>>2|sbus_buffer[21]<<6)                & 0x07FF);
		channels[15] = ((sbus_buffer[21]>>5|sbus_buffer[22]<<3)                & 0x07FF);

		if (sbus_buffer[23] & 0x01) {
			channels[16] = 2047;
		} else {
			channels[16] = 0;
		}
		if (sbus_buffer[23] & 0x02) {
			channels[17] = 2047;
		} else {
			channels[17] = 0;
		}

//		if ((sbus_buffer[23] >> 3) & 0x0001) {
//			_failsafe = SBUS_FAILSAFE_ACTIVE;
//		} else {
//			_failsafe = SBUS_FAILSAFE_INACTIVE;
//		}
//
//		if ((buffer[23] >> 2) & 0x0001) {
//			_lostFrames++;
//		}
	}
	return SBUS_FRAME_PARSING;
}
